﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/Shapes/PolygonShape.h
// Revision   : r132
// Change-Date: 2010-09-21
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/Shapes/PolygonShape.cpp
// Revision   : r132
// Change-Date: 2010-09-21
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// A convex polygon. It is assumed that the interior of the polygon is to 
    /// the left of each edge.<br/>
    /// Polygons have a maximum number of vertices equal to PhysicSettings.MaxPolygonVertices.
    /// In most cases you should not need many vertices for a convex polygon.
    /// </summary>
    public class PolygonShape : Shape
    {
        static float k_inv3 = 1.0f / 3.0f;
        
        public PolygonShape() 
        { 
            m_type = ShapeType.POLYGON; 
            m_radius = PhysicSettings.PolygonRadius;
            m_vertexCount = 0;
            m_centroid = Vector2.Zero;
        }

        public override Shape Clone()
        {
            var clone = new PolygonShape();
            clone.m_type = m_type;
            clone.m_radius = m_radius;
            clone.m_vertexCount = m_vertexCount;
            clone.m_centroid = m_centroid;
            clone.m_vertices = (Vector2[])m_vertices.Clone();
            clone.m_normals = (Vector2[])m_normals.Clone(); ;

            return clone;
        }

        public override int GetChildCount()
        {
            return 1;
        }

        /// <summary>
        /// Copy vertices. This assumes the vertices define a convex polygon.
	    /// It is assumed that the exterior is the the right of each edge.
        /// </summary>
        /// <remarks>
        /// The count must be in the range [3,PhysicSettings.MaxPolygonVertices]
        /// </remarks>
	    public void Set(Vector2[] vertices, int vertexCount)
        {
            System.Diagnostics.Debug.Assert(3 <= vertexCount && vertexCount <= PhysicSettings.MaxPolygonVertices);

            m_vertexCount = vertexCount;

	        // Copy vertices.
	        for (int i = 0; i < m_vertexCount; ++i)
	        {
		        m_vertices[i] = vertices[i];
	        }

	        // Compute normals. Ensure the edges have non-zero length.
	        for (int i = 0; i < m_vertexCount; ++i)
	        {
		        int i1 = i;
		        int i2 = i + 1 < m_vertexCount ? i + 1 : 0;
		        Vector2 edge = m_vertices[i2] - m_vertices[i1];
		        System.Diagnostics.Debug.Assert(edge.LengthSquared() > PhysicSettings.FLT_EPSILON_SQUARED);

                Vector2 v = Vector2Extensions.Cross(edge, 1.0f);
                v.Normalize();

                m_normals[i] = v;
	        }

#if _DEBUG
	        // Ensure the polygon is convex and the interior
	        // is to the left of each edge.
	        for (int i = 0; i < m_vertexCount; ++i)
	        {
		        int i1 = i;
		        int i2 = i + 1 < m_vertexCount ? i + 1 : 0;
		        Vector2 edge = m_vertices[i2] - m_vertices[i1];

		        for (int j = 0; j < m_vertexCount; ++j)
		        {
			        // Don't check vertices on the current edge.
			        if (j == i1 || j == i2)
			        {
				        continue;
			        }
        			
			        Vector2 r = m_vertices[j] - m_vertices[i1];

			        // Your polygon is non-convex (it has an indentation) or
			        // has colinear edges.
                    float s = edge.Cross(r);
			        System.Diagnostics.Debug.Assert(s > 0.0f);
		        }
	        }
#endif

	        // Compute the polygon centroid.
	        m_centroid = ComputeCentroid(ref m_vertices, m_vertexCount);
        }

        /// <summary>Build vertices to represent an axis-aligned box.</summary>
	    /// <param name="hx">the half-width.</param>
	    /// <param name="hy">the half-height.</param>
	    public void SetAsBox(float hx, float hy)
        {
            m_vertexCount = 4;
            m_vertices[0] = new Vector2(-hx, -hy);
            m_vertices[1] = new Vector2(hx, -hy);
            m_vertices[2] = new Vector2(hx,  hy);
            m_vertices[3] = new Vector2(-hx,  hy);
            m_normals[0] = new Vector2(0.0f, -1.0f);
            m_normals[1] = new Vector2(1.0f, 0.0f);
            m_normals[2] = new Vector2(0.0f, 1.0f);
            m_normals[3] = new Vector2(-1.0f, 0.0f);
            m_centroid = Vector2.Zero;
        }

        /// <summary>Build vertices to represent an oriented box.</summary>
	    /// <param name="hx">the half-width.</param>
	    /// <param name="hy">the half-height.</param>
	    /// <param name="center">the center of the box in local coordinates.</param>
	    /// <param name="angle">the rotation of the box in local coordinates.</param>
	    public void SetAsBox(float hx, float hy, Vector2 center, float angle)
        {
            m_vertexCount = 4;
            m_vertices[0] = new Vector2(-hx, -hy);
            m_vertices[1] = new Vector2(hx, -hy);
            m_vertices[2] = new Vector2(hx, hy);
            m_vertices[3] = new Vector2(-hx, hy);
            m_normals[0] = new Vector2(0.0f, -1.0f);
            m_normals[1] = new Vector2(1.0f, 0.0f);
            m_normals[2] = new Vector2(0.0f, 1.0f);
            m_normals[3] = new Vector2(-1.0f, 0.0f);
            m_centroid = center;

            Transform xf = new Transform();
            xf.position = center;
            xf.R.Set(angle);

            // Transform vertices and normals. 
            for (Int32 i = 0; i < m_vertexCount; ++i)
            {
#if !OPTIMIZE
                m_vertices[i] = Math.Mul(xf, m_vertices[i]);
                m_normals[i] = Math.Mul(xf.R, m_normals[i]);
#else
                Vector2 mvi = m_vertices[i];
                Vector2 t; t.X = xf.R.col1.X * mvi.X + xf.R.col2.X * mvi.Y; t.Y = xf.R.col1.Y * mvi.X + xf.R.col2.Y * mvi.Y;
                m_vertices[i].X = xf.position.X + t.X;
                m_vertices[i].Y = xf.position.Y + t.Y;

                Vector2 tmni; tmni.X = xf.R.col1.X * m_normals[i].X + xf.R.col2.X * m_normals[i].Y; tmni.Y = xf.R.col1.Y * m_normals[i].X + xf.R.col2.Y * m_normals[i].Y;
                m_normals[i].X = tmni.X; m_normals[i].Y = tmni.Y;
#endif
            }
        }

        /// <summary>Test a point for containment in this shape. This only works for convex shapes.</summary>
        /// <param name="p">point in world coordinates</param>
        /// <param name="xf">the shape world transform</param>
        public override bool TestPoint(ref Transform transform, Vector2 p)
        {
#if !OPTIMIZE
            Vector2 pLocal = Math.MulT(transform.R, p - transform.position);

            for (Int32 i = 0; i < m_vertexCount; ++i)
            {
                float dot = Vector2.Dot(m_normals[i], pLocal - m_vertices[i]);
                if (dot > 0.0f) return false;
            }
#else
            Vector2 pLocal;
            Vector2 t; t.X = p.X - transform.position.X; t.Y = p.Y - transform.position.Y;
            pLocal.X = t.X * transform.R.col1.X + t.Y * transform.R.col1.Y; // Vector2.Dot(ref t, ref transform.R.col1, out pLocal.X);
            pLocal.Y = t.X * transform.R.col2.X + t.Y * transform.R.col2.Y; // Vector2.Dot(ref t, ref transform.R.col2, out pLocal.Y);

            float dot;
            for (Int32 i = 0; i < m_vertexCount; ++i)
            {
                t.X = pLocal.X - m_vertices[i].X;
                t.Y = pLocal.Y - m_vertices[i].Y;
                dot = m_normals[i].X * t.X + m_normals[i].Y * t.Y; // Vector2.Dot(ref m_normals[i], ref t, out dot);
                if (dot > 0.0f) return false;
            }
#endif

            return true;
        }

        /// <summary>Cast a ray against this shape. </summary>
        /// <param name="output">the ray-cast results. </param> 
        /// <param name="input">the ray-cast input parameters. </param> 
        /// <param name="transform">the transform to be applied to the shape. </param> 
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform xf, int childIndex)
        {
            output = new RayCastOutput();

            // Put the ray into the polygon's frame of reference.
#if !OPTIMIZE
            Vector2 t1;
            Vector2 p1 = Math.MulT(xf.R, input.p1 - xf.position);
            Vector2 p2 = Math.MulT(xf.R, input.p2 - xf.position);
#else
            Vector2 p1, p2, t1;

            t1 = input.p1 - xf.position;
            p1.X = t1.X * xf.R.col1.X + t1.Y * xf.R.col1.Y; // Vector2.Dot(ref t1, ref xf.R.col1, out p1.X);
            p1.Y = t1.X * xf.R.col2.X + t1.Y * xf.R.col2.Y; // Vector2.Dot(ref t1, ref xf.R.col2, out p1.Y);

            t1 = input.p2 - xf.position;
            Vector2.Dot(ref t1, ref xf.R.col1, out p2.X);
            Vector2.Dot(ref t1, ref xf.R.col2, out p2.Y);
#endif
            Vector2 d; d.X = p2.X - p1.X; d.Y = p2.Y - p1.Y;

            float lower = 0.0f, upper = input.maxFraction;

            int index = -1;

            for (int i = 0 ; i < m_vertexCount; ++i)
            {
                // p = p1 + a * d
                // dot(normal, p - v) = 0
                // dot(normal, p1 - v1) + a * dot(normal, d) = 0
#if !OPTIMIZE
                float numerator = Vector2.Dot(m_normals[i], m_vertices[i] - p1);
                float denominator = Vector2.Dot(m_normals[i], d);
#else
                t1.X = m_vertices[i].X - p1.X; t1.Y = m_vertices[i].Y - p1.Y;
                float numerator = m_normals[i].X * t1.X + m_normals[i].Y * t1.Y; // Vector2.Dot(ref normal, ref t1, out numerator);
                float denominator = m_normals[i].X * d.X + m_normals[i].Y * d.Y; // Vector2.Dot(ref normal, ref d, out denominator);
#endif

                if (denominator == 0.0f)
                {
                    if (numerator < 0.0f)
                    {
                        return false;
                    }
                }
                else
                {
                    // Note: we want this predicate without division:
                    // lower < numerator / denominator, where denominator < 0
                    // Since denominator < 0, we have to flip the inequality:
                    // lower < numerator / denominator <==> denominator * lower > numerator.
                    if (denominator < 0.0f && numerator < lower * denominator)
                    {
                        // Increase lower.
                        // The segment enters this half-space.
                        lower = numerator / denominator;
                        index = i;
                    }
                    else if (denominator > 0.0f && numerator < upper * denominator)
                    {
                        // Decrease upper.
                        // The segment exits this half-space.
                        upper = numerator / denominator;
                    }
                }

                // The use of epsilon here causes the assert on lower to trip
                // in some cases. Apparently the use of epsilon was to make edge
                // shapes work, but now those are handled separately.
                // if (upper < lower - b2_epsilon)
                if (upper < lower)
                {
                    return false;
                }
            }

            System.Diagnostics.Debug.Assert(0.0f <= lower && lower <= input.maxFraction);

            if (index >= 0)
            {
                output.fraction = lower;
                output.normal = Math.Mul(xf.R, m_normals[index]);
                return true;
            }

            return false;
        }

        /// <summary>Given a transform, compute the associated axis aligned bounding box for this shape.</summary>
        /// <param name="aabb">returns the axis aligned box.</param> 
        /// <param name="xf">the world transform of the shape.</param> 
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            aabb = AABB.Empty;

#if !OPTIMIZE
            Vector2 lower = Math.Mul(transform, m_vertices[0]);
#else
            Vector2 lower = Vector2.Zero;
            Vector2 v0 = m_vertices[0];
            lower.X = transform.position.X + transform.R.col1.X * v0.X + transform.R.col2.X * v0.Y;
            lower.Y = transform.position.Y + transform.R.col1.Y * v0.X + transform.R.col2.Y * v0.Y;
#endif
            Vector2 upper = lower;

            for (Int32 i = 1; i < m_vertexCount; ++i)
            {
#if !OPTIMIZE
                Vector2 v = Math.Mul(transform, m_vertices[i]);
                lower = Vector2.Min(lower, v);
                upper = Vector2.Max(upper, v);
            }

            Vector2 r; r.X = m_radius; r.Y = m_radius;
            aabb.lowerBound = lower - r;
            aabb.upperBound = upper + r;
#else
                Vector2 vi = m_vertices[i];
                Vector2 v;
                v.X = transform.position.X + transform.R.col1.X * vi.X + transform.R.col2.X * vi.Y;
                v.Y = transform.position.Y + transform.R.col1.Y * vi.X + transform.R.col2.Y * vi.Y;
                lower.X = lower.X < v.X ? lower.X : v.X;
                lower.Y = lower.Y < v.Y ? lower.Y : v.Y;
                upper.X = upper.X > v.X ? upper.X : v.X;
                upper.Y = upper.Y > v.Y ? upper.Y : v.Y;
            }

            Vector2 r; r.X = m_radius; r.Y = m_radius;
            aabb.lowerBound.X = lower.X - r.X;
            aabb.lowerBound.Y = lower.Y - r.Y;
            aabb.upperBound.X = upper.X + r.X;
            aabb.upperBound.Y = upper.Y + r.Y;
#endif
        }

        /// <summary>Compute the mass properties of this shape using its dimensions and density.
        /// The inertia tensor is computed about the local origin, not the centroid.</summary>
        /// <param name="massData">returns the mass data for this shape.</param> 
        /// <param name="density">the density in kilograms per meter squared.</param> 
        public override void ComputeMass(out MassData massData, float density)
        {
            // Polygon mass, centroid, and inertia.
            // Let rho be the polygon density in mass per unit area.
            // Then:
            // mass = rho * int(dA)
            // centroid.x = (1/mass) * rho * int(x * dA)
            // centroid.y = (1/mass) * rho * int(y * dA)
            // I = rho * int((x*x + y*y) * dA)
            //
            // We can compute these integrals by summing all the integrals
            // for each triangle of the polygon. To evaluate the integral
            // for a single triangle, we make a change of variables to
            // the (u,v) coordinates of the triangle:
            // x = x0 + e1x * u + e2x * v
            // y = y0 + e1y * u + e2y * v
            // where 0 <= u && 0 <= v && u + v <= 1.
            //
            // We integrate u from [0,1-v] and then v from [0,1].
            // We also need to use the Jacobian of the transformation:
            // D = cross(e1, e2)
            //
            // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
            //
            // The rest of the derivation is handled by computer algebra.

            System.Diagnostics.Debug.Assert(m_vertexCount >= 3);

            Vector2 center = Vector2.Zero;
            float area = 0.0f;
            float I = 0.0f;

            // pRef is the reference point for forming triangles.
            // It's location doesn't change the result (except for rounding error).
            Vector2 pRef = Vector2.Zero;
            //#if 0
            //    // This code would put the reference point inside the polygon.
            //    for (int32 i = 0; i < m_vertexCount; ++i)
            //    {
            //        pRef += m_vertices[i];
            //    }
            //    pRef *= 1.0f / count;
            //#endif

            for (int i = 0; i < m_vertexCount; ++i)
            {
                // Triangle vertices.
                Vector2 p1 = pRef;
                Vector2 p2 = m_vertices[i];
                Vector2 p3 = i + 1 < m_vertexCount ? m_vertices[i + 1] : m_vertices[0];

                Vector2 e1 = p2 - p1;
                Vector2 e2 = p3 - p1;

                float D = Vector2Extensions.Cross(e1, e2);

                float triangleArea = 0.5f * D;
                area += triangleArea;

                // Area weighted centroid
                center += triangleArea * k_inv3 * (p1 + p2 + p3);

                float px = p1.X, py = p1.Y;
                float ex1 = e1.X, ey1 = e1.Y;
                float ex2 = e2.X, ey2 = e2.Y;

                float intx2 = k_inv3 * (0.25f * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) + (px * ex1 + px * ex2)) + 0.5f * px * px;
                float inty2 = k_inv3 * (0.25f * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) + (py * ey1 + py * ey2)) + 0.5f * py * py;

                I += D * (intx2 + inty2);
            }

            // Total mass
            massData.mass = density * area;

            // Center of mass
            System.Diagnostics.Debug.Assert(area > PhysicSettings.FLT_EPSILON);
            center *= 1.0f / area;
            massData.center = center;

            // Inertia tensor relative to the local origin.
            massData.I = density * I;
        }

        /// <summary>
        /// Get the supporting vertex index in the given direction.
        /// </summary>
        //public int GetSupport(Vector2 d)
        //{
        //    int bestIndex = 0;
        //    float bestValue = Vector2.Dot(m_vertices[0], d);
        //    for (Int32 i = 1; i < m_vertexCount; ++i)
        //    {
        //        float value = Vector2.Dot(m_vertices[i], d);
        //        if (value > bestValue)
        //        {
        //            bestIndex = i;
        //            bestValue = value;
        //        }
        //    }

        //    return bestIndex;
        //}

        /// <summary>
        /// Get the supporting vertex in the given direction.
        /// </summary>
        //public Vector2 GetSupportVertex(Vector2 d)
        //{
        //    int bestIndex = 0;
        //    float bestValue = Vector2.Dot(m_vertices[0], d);
        //    for (int i = 1; i < m_vertexCount; ++i)
        //    {
        //        float value = Vector2.Dot(m_vertices[i], d);
        //        if (value > bestValue)
        //        {
        //            bestIndex = i;
        //            bestValue = value;
        //        }
        //    }

        //    return m_vertices[bestIndex];
        //}

        /// <summary>
        /// Get the vertex count.
        /// </summary>
        /// <returns></returns>
        public Int32 GetVertexCount()
        {
            return m_vertexCount;
        }

        /// <summary>
        /// Get a vertex by index.
        /// </summary>
        public Vector2 GetVertex(int index)
        {
            System.Diagnostics.Debug.Assert(0 <= index && index < m_vertexCount);
            return m_vertices[index];
        }

        static float inv3 = 1.0f / 3.0f;
        public static Vector2 ComputeCentroid(ref Vector2[] vs, int count)
        {
            System.Diagnostics.Debug.Assert(count >= 3);

            Vector2 c = Vector2.Zero;
            float area = 0.0f;

            // pRef is the reference point for forming triangles.
            // It's location doesn't change the result (except for rounding error).
            Vector2 pRef = Vector2.Zero;
            //#if 0
            //    // This code would put the reference point inside the polygon.
            //    for (int32 i = 0; i < count; ++i)
            //    {
            //        pRef += vs[i];
            //    }
            //    pRef *= 1.0f / count;
            //#endif

            //float inv3 = 1.0f / 3.0f;

            for (int i = 0; i < count; ++i)
            {
                // Triangle vertices.
                Vector2 p1 = pRef;
                Vector2 p2 = vs[i];
                Vector2 p3 = i + 1 < count ? vs[i + 1] : vs[0];

                Vector2 e1 = p2 - p1;
                Vector2 e2 = p3 - p1;

                float D = Vector2Extensions.Cross(e1, e2);

                float triangleArea = 0.5f * D;
                area += triangleArea;

                // Area weighted centroid
                c += triangleArea * inv3 * (p1 + p2 + p3);
            }

            // Centroid
            System.Diagnostics.Debug.Assert(area > PhysicSettings.FLT_EPSILON);
            c *= 1.0f / area;
            return c;
        }

	    public Vector2 m_centroid;
	    public Vector2[] m_vertices = new Vector2[PhysicSettings.MaxPolygonVertices];
	    public Vector2[] m_normals = new Vector2[PhysicSettings.MaxPolygonVertices];
        public int m_vertexCount;
    };
}
